﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  class GradientDescentTool
  {
    double xDiff = 2;
    double yDiff = 2;
    double angleDiff = 1;
    double startingAngleDiff = 0.5;

    WorldState lastStartPosition = null;

    public WorldState DoGradientDescent2(WorldState startPosition, Experiment experiment)
    {
      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);

      int numberOfIterations = 80;

      Init(startPosition);

      for (int i = 0; i < numberOfIterations; i++)
      {
        WorldState oldPosition = startPosition;
        WorldState gradient = CalculateGradient2(listOfOldPlaces, startPosition, experiment);
        startPosition.Move(gradient);
        startPosition = gradient;
        int intensity = (int)((numberOfIterations - i) * 200.0 / numberOfIterations);
        Color color = Color.FromArgb(200 - intensity, 0, intensity);
        startPosition.DrawHighlighted("world:GDTool:gd steps", color);
        if (oldPosition is WorldState2D)
        {
          DebugDrawingTool.CreateLine("world:MBL:gradient descent path", 
            (oldPosition as WorldState2D).Vector, 
            (startPosition as WorldState2D).Vector, 4, color);
        }
      }
      return (WorldState)startPosition.Clone();
    }
    private WorldState CalculateGradient2(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, WorldState position, Experiment setup)
    {
      if (position is WorldState1D)
      {
        double x = (position as WorldState1D).Position;
        double lowX = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldState1D(x - 1), setup);
        double highX = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldState1D(x + 1), setup);

        return new WorldState1D(lowX - highX);
      }
      else if (position is WorldState2D)
      {
        double x = (position as WorldState2D).Vector.x;
        double y = (position as WorldState2D).Vector.y;
        double lowX = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldState2D(x - 1, y), setup);
        double highX = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldState2D(x + 1, y), setup);
        double lowY = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldState2D(x, y - 1), setup);
        double highY = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldState2D(x, y + 1), setup);
        Vector2 vector = new Vector2(lowX - highX, lowY - highY);
        vector.Normalize();
        return new WorldState2D(5.0 * vector.x, 5.0 * vector.y);
      }
      else if (position is WorldStatePose2D)
      {
        double x = (position as WorldStatePose2D).Pose.translation.x;
        double y = (position as WorldStatePose2D).Pose.translation.y;
        double angle = (position as WorldStatePose2D).Pose.rotation;

        WorldStatePose2D positionOfMinimalValue;
        positionOfMinimalValue = FindPositionOfMinimalValue(listOfOldPlaces, setup, x, y, angle, xDiff, yDiff, angleDiff);
        if (
          positionOfMinimalValue.Pose.rotation == (position as WorldStatePose2D).Pose.rotation &&
          positionOfMinimalValue.Pose.translation.x == (position as WorldStatePose2D).Pose.translation.x &&
          positionOfMinimalValue.Pose.translation.y == (position as WorldStatePose2D).Pose.translation.y
          )
        {
//          xDiff *= 0.9;
//          yDiff *= 0.9;
          angleDiff *= 0.7;
        }
        return positionOfMinimalValue;
        //        return new WorldStatePose2D( (position as WorldStatePose2D).Pose.minusDiff( (positionOfMinimalValue as WorldStatePose2D).Pose ) );
        /*
                double x = (position as WorldStatePose2D).Pose.translation.x;
                double y = (position as WorldStatePose2D).Pose.translation.y;
                double angle = (position as WorldStatePose2D).Pose.rotation;
                double lowX = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldStatePose2D(x - 1, y, angle), setup);
                double highX = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldStatePose2D(x + 1, y, angle), setup);
                double lowY = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldStatePose2D(x, y - 1, angle), setup);
                double highY = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldStatePose2D(x, y + 1, angle), setup);
                double lowAngle = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldStatePose2D(x, y, angle - 0.001), setup);
                double highAngle = MBLTools.CalculateFunction2(listOfOldPlaces, new WorldStatePose2D(x, y, angle + 0.001), setup);
                Vector2 vector = new Vector2(lowX - highX, lowY - highY);
        //        double length = vector.Abs();
                vector.Normalize();
                vector *= 10.0;
                double angleDiff = -Math.Sign(lowAngle - highAngle) * 0.0001;
        //        angleDiff = 0;
                return new WorldStatePose2D(vector.x, vector.y, angleDiff);
         */
      }
      else
        return null;
    }
    private WorldStatePose2D FindPositionOfMinimalValue(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, Experiment setup, double x, double y, double angle, double xDiff, double yDiff, double angleDiff)
    {
      double minValue = double.MaxValue;
      double maxValue = double.MinValue;
      WorldStatePose2D positionOfMinimalValue = new WorldStatePose2D();
      WorldStatePose2D positionOfMaximalValue = new WorldStatePose2D();

      for (double xOffset = -xDiff; xOffset <= xDiff; xOffset += xDiff)
      {
        for (double yOffset = -yDiff; yOffset <= yDiff; yOffset += yDiff)
        {
          for (double angleOffset = -angleDiff; angleOffset <= angleDiff; angleOffset += angleDiff)
          {
            WorldStatePose2D newState = new WorldStatePose2D(x + xOffset, y + yOffset, angle + angleOffset);
            newState.DrawHighlighted("world:GDTool:show matrix", Color.Red);
            double value = MBLTools.CalculateFunction2(listOfOldPlaces, newState, setup);
            if (value > maxValue)
            {
              maxValue = value;
              positionOfMaximalValue = newState;
            }
            if (value < minValue)
            {
              minValue = value;
              positionOfMinimalValue = newState;
            }
          }
        }
      }
      return positionOfMinimalValue;
    }
    private void Init(WorldState startPosition)
    {
      xDiff = 10.0;
      yDiff = 10.0;

      if (DebugKeyTool.IsActive("GD:reset starting angle diff")) startingAngleDiff = 0.5;

      if (lastStartPosition != null)
      {
        double movedDistance;
        double turnedAngle;
        if(lastStartPosition is WorldStatePose2D)
        {
          movedDistance = 
            ((lastStartPosition as WorldStatePose2D).Pose.translation - 
            (startPosition as WorldStatePose2D).Pose.translation).Abs();
          turnedAngle =
            Math.Abs((lastStartPosition as WorldStatePose2D).Pose.rotation -
            (startPosition as WorldStatePose2D).Pose.rotation);
          if (turnedAngle == 0)
          {
            if (startingAngleDiff < 1.0)
              startingAngleDiff += 0.1;
          }
          if (movedDistance < 1)
          {
            xDiff = 1;
            yDiff = 1;
          }
          else if (movedDistance >= 100)
          {
            xDiff = 10;
            yDiff = 10;
          }
          else
          {
            xDiff = movedDistance / 10.0;
            yDiff = movedDistance / 10.0;
          }
        }
      }
      angleDiff = startingAngleDiff;
      if (DebugKeyTool.IsActive("GD:freeze stepLength"))
      {
        angleDiff = 0.01;
        xDiff = 2;
        yDiff = 2;
      }
      lastStartPosition = startPosition;
    }
  }
}
